package de.fhin.mas.agents.navigator.behaviours;

import jade.core.Agent;
import jade.core.behaviours.TickerBehaviour;
import de.fhin.mas.agents.navigator.NavigatorAgent;
import de.fhin.mas.util.Coordinate;

/**
 * 
 * @author Matthias Wilhelm
 * 
 */
public class UpdatePosition extends TickerBehaviour {
	private int xSpeed = 0;
	private int ySpeed = 0;
	private final int maxSpeed;

	public UpdatePosition(Agent a, long period) {
		super(a, period);
		maxSpeed = 20;
	}

	/**
	 * 
	 */
	private static final long serialVersionUID = 7147695988212939477L;

	private int maxSpeed(int speed) {
		if (speed > maxSpeed) {
			speed = 20;
		} else if (speed < -maxSpeed) {
			speed = -20;
		}
		return speed;
	}

	private int breakPoint(int speed) {
		int breakPoint = 0;
		if (speed > 0)
			while (speed > 0) {
				breakPoint = breakPoint + speed;
				speed--;
			}
		else {
			while (speed < 0) {
				breakPoint = breakPoint + speed;
				speed++;
			}
		}
		return breakPoint;
	}

	@Override
	protected void onTick() {
		// TODO: Wird normalerweise vom Roboter übernommen
		// Da keine Hardwareimplementierung stattfindet muss sich mit DummyDaten
		// beholfen werden
		Coordinate position = ((NavigatorAgent) myAgent).get_myPosition();
		Coordinate destination = ((NavigatorAgent) myAgent).get_myDestination();

		int xDistance = destination.getX() - position.getX();
		int yDistance = destination.getY() - position.getY();
		if (Math.abs(xDistance) > 0) {
			if (xDistance > breakPoint(xSpeed)) {
				// || xDistance > -breakPoint(xSpeed) && xDistance < 0) {
				if (xSpeed > 10) {
					xSpeed = xSpeed + 2;
				} else {
					xSpeed++;
				}
			} else if (xDistance < -breakPoint(xSpeed)) {
				// || xDistance < breakPoint(xSpeed) && xDistance > 0) {
				if (xSpeed < -10) {
					xSpeed = xSpeed - 2;
				} else {
					xSpeed--;
				}

			}
		}
		if (Math.abs(yDistance) > 0) {
			if (yDistance > breakPoint(ySpeed)
					|| yDistance > -breakPoint(ySpeed) && yDistance < 0) {
				if (ySpeed > 10) {
					ySpeed = ySpeed + 2;
				} else {
					ySpeed++;
				}
			} else if (yDistance < -breakPoint(ySpeed)
					|| yDistance < breakPoint(ySpeed) && yDistance > 0) {
				if (ySpeed < -10) {
					ySpeed = ySpeed - 2;
				} else {
					ySpeed--;
				}

			}
		}
		xSpeed = maxSpeed(xSpeed);
		ySpeed = maxSpeed(ySpeed);
		((NavigatorAgent) myAgent).set_myPosition(new Coordinate(position
				.getX()
				+ xSpeed, position.getY() + ySpeed));
		((NavigatorAgent) myAgent).println("@ "
				+ ((NavigatorAgent) myAgent).get_myPosition().toString() + " "
				+ ((NavigatorAgent) myAgent).get_myDirection()
				+ "°. Driving to "
				+ ((NavigatorAgent) myAgent).get_myDestination().toString()
				+ ".");
	}
}
